/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 *
 * JointPos.c
 *
 * Code generation for function 'JointPos'
 *
 */

/* Include files */
#include "JointPos.h"
#include <math.h>

/* Function Definitions */
void JointPos(float l0, float phi0, float pos[2])
{
  float t3;
  float t4;
  float t8;
  float t8_tmp;
  float t9;
  /* JointPos */
  /*     POS = JointPos(L0,PHI0) */
  /*     This function was generated by the Symbolic Math Toolbox version 9.3.
   */
  /*     2023-10-21 16:48:46 */
  t3 = l0 * l0;
  t4 = l0 * cos(phi0) * 0.095;
  t8_tmp = t3 + t4;
  t8 = 1.0 / sqrt(t8_tmp + 0.00225625);
  t9 = 1.0 / sqrt((t3 - t4) + 0.00225625);
  pos[0] = acos(t8 * (t4 + 0.0045125) * 10.526315789473685) +
           acos(t8 * (t8_tmp - 0.02481875) * 5.2631578947368425);
  pos[1] =
      (acos(t9 * (t4 - 0.0045125) * 10.526315789473685) - 3.1415926535897931) +
      acos(t9 * ((-t3 + t4) + 0.02481875) * 5.2631578947368425);
}

/* End of code generation (JointPos.c) */
